
#ifndef _vector3_h
#define _vector3_h

#include <cmath>
#include <cassert>

template <typename Real>
class Vector3
{
public:
  typedef Real ValueType;

public:
  // Constructors, destructor and assignment operators
  // Default constructor, initializes to 0
  Vector3() {
    v[0] = v[1] = v[2] = Real(0.0);
  }
  Vector3(Real v0) {
    v[0] = v[1] = v[2] = v0;
  }
  Vector3(Real v0, Real v1, Real v2) {
    v[0] = v0;
    v[1] = v1;
    v[2] = v2;
  }

  //Copy constructor
  template <typename Real2>
  Vector3(const Vector3<Real2>& vec3) {
    *this = vec3;
  }
  //Assignment operator = for vector
  template <typename Real2>
  Vector3& operator=(const Vector3<Real2>& vec3) {
    v[0]=(Real)vec3[0];
    v[1]=(Real)vec3[1];
    v[2]=(Real)vec3[2];
    return *this;
  }
  //Assignment operator = for scalar
  template <typename Scalar>
  Vector3& operator=(Scalar s) {
    v[0]=(Real)s;
    v[1]=(Real)s;
    v[2]=(Real)s;
    return *this;
  }

  // Destructor
  ~Vector3() { }


  // Access operators
  Real& operator[](unsigned int i) {
    assert(i >= 0 && i <= 2 && "Index out of bounds in Vector3");
    return v[i];
  }
  Real operator[](unsigned int i) const {
    assert(i >= 0 && i <= 2 && "Index out of bounds in Vector3");
    return v[i];
  }


  // Algebraic operators
  Vector3 operator-(const Vector3& vec3) const {
    return Vector3(v[0]-vec3.v[0], v[1]-vec3.v[1], v[2]-vec3.v[2]);
  }
  Vector3 operator+(const Vector3& vec3) const {
    return Vector3(v[0]+vec3.v[0], v[1]+vec3.v[1], v[2]+vec3.v[2]);
  }
  Vector3 & operator+=(const Vector3& vec3) {
    v[0]+=vec3.v[0];
    v[1]+=vec3.v[1];
    v[2]+=vec3.v[2];
    return *this;
  }
  Vector3 & operator-=(const Vector3& vec3) {
    v[0]-=vec3.v[0];
    v[1]-=vec3.v[1];
    v[2]-=vec3.v[2];
    return *this;
  }
  // Dot product
  Real operator*(const Vector3<Real>& vec3) const {
    return v[0]*vec3[0] + v[1]*vec3[1] + v[2]*vec3[2];
  }

  // Scalar multiply
  template<typename Scalar>
  Vector3<Real> operator*(Scalar s) const {
    return Vector3<Real>((Real)(s*v[0]), (Real)(s*v[1]), (Real)(s*v[2]));
  }
  template<typename Scalar>
  Vector3<Real>& operator*=(Scalar s) {
    v[0]*=(Real)s;
    v[1]*=(Real)s;
    v[2]*=(Real)s;
    return *this;
  }



  //Length of a vector
  Real Length() const {
    return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
  }
  //Normalization of vector
  Vector3& Normalize() {
    Real len = Length();
    v[0] /= len;
    v[1] /= len;
    v[2] /= len;
    return *this;
  }


protected:
  Real v[3];
  // friends
  // Cross product
  template<typename Real2>
  friend Vector3<Real2> Cross(const Vector3<Real2>& v1, const Vector3<Real2>& v2);
};

// Cross product
template <typename Real2>
Vector3<Real2> Cross(const Vector3<Real2>& v1, const Vector3<Real2>& v2)
{
  Vector3<Real2> res;

  res[0] = v1[1] * v2[2] - v1[2] * v2[1];
  res[1] = v1[2] * v2[0] - v1[0] * v2[2];
  res[2] = v1[0] * v2[1] - v1[1] * v2[0];

  return res;
}


template <typename Real1, typename Real2>
Vector3<Real2> operator*(Real1 r, const Vector3<Real2>& v)
{
  Vector3<Real2> res;

  res[0] = v[0]*r;
  res[1] = v[1]*r;
  res[2] = v[2]*r;

  return res;
}

template <typename Real>
const Vector3<Real> operator-(const Vector3<Real>& vec3)
{
  return Vector3<Real>(-vec3[0], -vec3[1], -vec3[2]);
}


#endif
